﻿
Import mojo
Import agfx

Class TCollisionResult
	Field WillIntersect:Bool = False
	Field Intersect:Bool = False
	Field MinimumTranslationVector:TVector2 = New TVector2()
	Field normal:TVector2 = New TVector2()
End Class
		
Class TCollisionMath
	
	Method TFormPolyToTFormPoly:Bool( p1_xy:Float[], p1_x:Float=0, p1_y:Float=0,p1_rot:Float=0, p1_scale_x:Float=1, p1_scale_y:Float=1,p1_handle_x:Float=0, p1_handle_y:Float=0,p1_origin_x:Float=0, p1_origin_y:Float=0,p2_xy:Float[], p2_x:Float=0, p2_y:Float=0,p2_rot:Float=0, p2_scale_x:Float=1, p2_scale_y:Float=1,p2_handle_x:Float=0, p2_handle_y:Float=0,p2_origin_x:Float=0, p2_origin_y:Float=0)
		
		If p1_xy.Length<6 Or (p1_xy.Length&1) Return False
		If p2_xy.Length<6 Or (p2_xy.Length&1) Return False
		
		Local tform1_xy:Float[]=TFormPoly(p1_xy,p1_x,p1_y,p1_rot,p1_scale_x,p1_scale_y,
							p1_handle_x,p1_handle_y,p1_origin_x,p1_origin_y)
		
		Local tform2_xy:Float[]=TFormPoly(p2_xy,p2_x,p2_y,p2_rot,p2_scale_x,p2_scale_y,
							p2_handle_x,p2_handle_y,p2_origin_x,p2_origin_y)
		
		If PolyToPoly(tform1_xy,tform2_xy)
			Return True
		Else
			Return False
		Endif
		
	End Method
	
	Method PolyToTFormPoly:Bool( p1_xy:Float[], p2_xy:Float[],p2_x:Float=0, p2_y:Float=0, rot:Float=0,scale_x:Float=1, scale_y:Float=1,handle_x:Float=0, handle_y:Float=0,origin_x:Float=0, origin_y:Float=0 )
		
		If p1_xy.Length<6 Or (p1_xy.Length&1) Return False
		If p2_xy.Length<6 Or (p2_xy.Length&1) Return False
		
		Local tform_xy:Float[]=TFormPoly(p2_xy,p2_x,p2_y,rot,scale_x,scale_y,
							handle_x,handle_y,origin_x,origin_y)
		
		If PolyToPoly(p1_xy,tform_xy)
			Return True
		Else
			Return False
		Endif
		
	End Method
	
	Method PolyToPoly:Bool( p1_xy:Float[], p2_xy:Float[] )
		
		If p1_xy.Length<6 Or (p1_xy.Length&1) Return False
		If p2_xy.Length<6 Or (p2_xy.Length&1) Return False
		
		For Local i:Int=0 Until p1_xy.Length Step 2
			If PointInPoly(p1_xy[i],p1_xy[i+1],p2_xy) Then Return True
		Next
		For Local i:Int=0 Until p2_xy.Length Step 2
			If PointInPoly(p2_xy[i],p2_xy[i+1],p1_xy) Then Return True
		Next
		
		Local l1_x1:Float=p1_xy[p1_xy.Length-2]
		Local l1_y1:Float=p1_xy[p1_xy.Length-1]
		For Local i1:Int=0 Until p1_xy.Length Step 2
			Local l1_x2:Float=p1_xy[i1]
			Local l1_y2:Float=p1_xy[i1+1]
			
			Local l2_x1:Float=p2_xy[p2_xy.Length-2]
			Local l2_y1:Float=p2_xy[p2_xy.Length-1]
			For Local i2:Int=0 Until p2_xy.Length Step 2
				Local l2_x2:Float=p2_xy[i2]
				Local l2_y2:Float=p2_xy[i2+1]
				
				If LinesCross(l1_x1,l1_y1,l1_x2,l1_y2,l2_x1,l2_y1,l2_x2,l2_y2)
					Return True
				Endif
				
				l2_x1=l2_x2
				l2_y1=l2_y2
			Next
			l1_x1=l1_x2
			l1_y1=l1_y2
		Next
		Return False
	End Method
	
	Method CircleToTFormPoly:Bool( circle_x:Float, circle_y:Float, radius:Float,xy:Float[], poly_x:Float=0, poly_y:Float=0, rot:Float=0,scale_x:Float=1, scale_y:Float=1,handle_x:Float=0, handle_y:Float=0,origin_x:Float=0, origin_y:Float=0 )
		
		If xy.Length<6 Or (xy.Length&1) Return False
		
		Local tform_xy:Float[]=TFormPoly(xy,poly_x,poly_y,rot,scale_x,scale_y,
							handle_x,handle_y,origin_x,origin_y)
		
		Return CircleToPoly(circle_x,circle_y,radius,tform_xy)
	End Method
	
	Method CircleToPoly:Bool( circle_x:Float, circle_y:Float, radius:Float, xy:Float[] )
		
		If xy.Length<6 Or (xy.Length&1) Return False
		
		If PointInPoly(circle_x,circle_y,xy) Then Return True
		
		Local x1:Float=xy[xy.Length-2]
		Local y1:Float=xy[xy.Length-1]
		
		For Local i:Int=0 Until xy.Length Step 2
			Local x2:Float=xy[i]
			Local y2:Float=xy[i+1]
			
			If LineToCircle(x1,y1,x2,y2,circle_x,circle_y,radius) Then Return True
			x1=x2
			y1=y2
		Next
		
		Return False
	End Method
	
	Method LineToTFormPoly:Bool( line_x1:Float, line_y1:Float, line_x2:Float, line_y2:Float,xy:Float[], poly_x:Float=0, poly_y:Float=0,rot:Float=0, scale_x:Float=1, scale_y:Float=1,handle_x:Float=0, handle_y:Float=0,origin_x:Float=0, origin_y:Float=0 )
		
		If xy.Length<6 Or (xy.Length&1) Return False
		
		TFormGlobalToLocal(line_x1,line_y1,poly_x,poly_y,rot,scale_x,
							scale_y,handle_x,handle_y,origin_x,origin_y)
		
		TFormGlobalToLocal(line_x2,line_y2,poly_x,poly_y,rot,scale_x,
							scale_y,handle_x,handle_y,origin_x,origin_y)
		
		Return LineToPoly(line_x1,line_y1,line_x2,line_y2,xy)
	End Method
	
	Method LineToPoly:Bool( line_x1:Float, line_y1:Float, line_x2:Float, line_y2:Float, xy:Float[] )
		
		If xy.Length<6 Or (xy.Length&1) Return False
		
		If PointInPoly(line_x1,line_y1,xy) Then Return True
		
		Local poly_x1:Float=xy[xy.Length-2]
		Local poly_y1:Float=xy[xy.Length-1]
		
		For Local i:Int=0 Until xy.Length Step 2
			Local poly_x2:Float=xy[i]
			Local poly_y2:Float=xy[i+1]
			
			If LinesCross(line_x1,line_y1,line_x2,line_y2,
							poly_x1,poly_y1,poly_x2,poly_y2) Then Return True
			poly_x1=poly_x2
			poly_y1=poly_y2
		Next
		
		Return False
		
	End Method
	
	Method PointInTFormPoly:Bool( point_x:Float, point_y:Float, xy:Float[],poly_x:Float=0, poly_y:Float=0, rot:Float=0,scale_x:Float=1, scale_y:Float=1,handle_x:Float=0, handle_y:Float=0,origin_x:Float=0, origin_y:Float=0 )
		
		If xy.Length<6 Or (xy.Length&1) Return False
		
		TFormGlobalToLocal(point_x,point_y,poly_x,poly_y,rot,scale_x,
							scale_y,handle_x,handle_y,origin_x,origin_y)
		
		Return PointInPoly(point_x,point_y,xy)
	End Method
	
	Method PointInPoly:Bool( point_x:Float, point_y:Float, xy:Float[] )
		
		If xy.Length<6 Or (xy.Length&1) Return False
		
		Local x1:Float=xy[xy.Length-2]
		Local y1:Float=xy[xy.Length-1]
		Local cur_quad:Int=GetQuad(point_x,point_y,x1,y1)
		Local next_quad:Int
		Local total:Int
		
		For Local i:Int=0 Until xy.Length Step 2
			Local x2:Float=xy[i]
			Local y2:Float=xy[i+1]
			next_quad=GetQuad(point_x,point_y,x2,y2)
			Local diff:Int=next_quad-cur_quad
			
			Select diff
			Case 2,-2
				If ( x2 - ( ((y2 - point_y) * (x1 - x2)) / (y1 - y2) ) )<point_x
					diff=-diff
				Endif
			Case 3
				diff=-1
			Case -3
				diff=1
			End Select
			
			total+=diff
			cur_quad=next_quad
			x1=x2
			y1=y2
		Next
		
		If Abs(total)=4 Then Return True Else Return False
	End Method
	
	Method TFormPoly:Float[]( xy:Float[], tform_x:Float=0, tform_y:Float=0, rot:Float=0,scale_x:Float=1, scale_y:Float=1,handle_x:Float=0, handle_y:Float=0,origin_x:Float=0, origin_y:Float=0 )
		
		If xy.Length<6 Or (xy.Length&1) Return Null
		
		Local tform_xy:Float[]=xy[..]
		
		For Local i:Int=0 Until tform_xy.Length Step 2
			TFormLocalToGlobal(tform_xy[i],tform_xy[i+1],tform_x,tform_y,rot,
							scale_x,scale_y,handle_x,handle_y,origin_x,origin_y)
		Next
		
		Return tform_xy
	End Method
	
	Method TFormGlobalToLocal:Void( point_x:Float[], point_y:Float[],tform_x:Float=0, tform_y:Float=0, rot:Float=0,scale_x:Float=1, scale_y:Float=1,handle_x:Float=0, handle_y:Float=0,origin_x:Float=0, origin_y:Float=0 )
		
		point_x[0]-=origin_x
		point_y[0]-=origin_y
		
		point_x[0]-=tform_x
		point_y[0]-=tform_y
		
		Local mag:Float=Sqr(point_x*point_x+point_y*point_y)
		Local ang:Float=ATan2(point_y,point_x)
		point_x[0]=Cos(ang-rot)*mag
		point_y[0]=Sin(ang-rot)*mag
		
		point_x[0]/=scale_x
		point_y[0]/=scale_y
		
		point_x[0]+=handle_x
		point_y[0]+=handle_y
	End Method
	
	Method TFormLocalToGlobal:Void( point_x:Float[], point_y:Float[],tform_x:Float=0, tform_y:Float=0, rot:Float=0,scale_x:Float=1, scale_y:Float=1,handle_x:Float=0, handle_y:Float=0,origin_x:Float=0, origin_y:Float=0 )
		
		point_x[0]-=handle_x
		point_y[0]-=handle_y
		
		point_x[0]*=scale_x
		point_y[0]*=scale_y
		
		Local mag:Float=Sqr(point_x*point_x+point_y*point_y)
		Local ang:Float=ATan2(point_y,point_x)
		point_x[0]=Cos(ang+rot)*mag
		point_y[0]=Sin(ang+rot)*mag
		
		point_x[0]+=tform_x
		point_y[0]+=tform_y
		
		point_x[0]+=origin_x
		point_y[0]+=origin_y
	End Method
	
	'Adapted from Fredborg's code
	Method LinesCross:Bool(l0:TLine,l1:TLine )
		Local A1:TVector2 = l0.GetPoint(0)
		Local B1:TVector2 = l0.GetPoint(1)
		Local A2:TVector2 = l1.GetPoint(0)
		Local B2:TVector2 = l1.GetPoint(1)
		Return LinesCross(A1.x,A1.y,B1.x,B1.y,A2.x,A2.y,B2.x,B2.y)
	End Method
	
	Method LinesCross:Bool(x0:Float, y0:Float , x1:Float, y1:Float,x2:Float ,y2:Float, x3:Float, y3:Float )
		  
		Local n:Float=(y0-y2)*(x3-x2)-(x0-x2)*(y3-y2)
		Local d:Float=(x1-x0)*(y3-y2)-(y1-y0)*(x3-x2)
		
		If Abs(d) < 0.0001 
			' Lines are parallel!
			Return False
		Else
			' Lines might cross!
			Local Sn:Float=(y0-y2)*(x1-x0)-(x0-x2)*(y1-y0)
	
			Local AB:Float=n/d
			If AB>0.0 And AB<1.0
				Local CD:Float=Sn/d
				If CD>0.0 And CD<1.0
					' Intersection Point
					Local X:Float=x0+AB*(x1-x0)
			       	Local Y:Float=y0+AB*(y1-y0)
					Return True
				End If
			End If
		
			' Lines didn't cross, because the intersection was beyond the end points of the lines
		Endif
	
		' Lines do Not cross!
		Return False
	
	End Method
	
	Method LineToCircle:Bool( l0:TLine,c:TCircle)
		Local A1:TVector2 = l0.GetPoint(0)
		Local B1:TVector2 = l0.GetPoint(1)
		
		Return Self.LineToCircle(A1.x,A1.y,B1.x,B1.y,c.center.x,c.center.y,c.radius)		
	End Method
	
	'Adapted from TomToad's code
	Method LineToCircle:Bool( x1:Float, y1:Float, x2:Float, y2:Float, px:Float, py:Float, r:Float )
		
		Local sx:Float = x2-x1
		Local sy:Float = y2-y1
		
		Local q:Float = ((px-x1) * (x2-x1) + (py - y1) * (y2-y1)) / (sx*sx + sy*sy)
		
		If q < 0.0 Then q = 0.0
		If q > 1.0 Then q = 1.0
		
		Local cx:Float=(1-q)*x1+q*x2
		Local cy:Float=(1-q)*y1 + q*y2
		
		
		If PointToPointDist(px,py,cx,cy) < r
			
			Return True
			
		Else
			
			Return False
			
		Endif
	
		
	End Method 
	
	Method PointToPointDist:Float( a:TVector2,b:TVector2 )
		Return Self.PointToPointDist(a.x,a.y,b.x,b.y)
	End Method
	
	Method PointToPointDist:Float( x1:Float, y1:Float, x2:Float, y2:Float )
	
		Local dx:Float = x1-x2
		Local dy:Float = y1-y2
		
		Return Sqrt(dx*dx + dy*dy)
	End Method
	
	
	Method GetQuad:Int(axis_x:Float,axis_y:Float,vert_x:Float,vert_y:Float)
		If vert_x<axis_x
			If vert_y<axis_y
				Return 1
			Else
				Return 4
			Endif
		Else
			If vert_y<axis_y
				Return 2
			Else
				Return 3
			Endif	
		Endif
	
	End Method	
	
	#Rem
	Summary: project group shape to line 
	and return Min, Max range
	#End
	Method ProjectPolygon:Void( axis : TVector2, polygon:TPrimitive, min : Float[], max : Float[] )
	
		Local DotP : Float =  VecDot( axis, polygon.points.GetFirst() )
		
		min[ 0 ] = DotP
		max[ 0 ] = DotP
		
		For Local p:TVector2 = EachIn polygon.points		
			DotP =  VecDot( axis, p)
			min[ 0 ] = Min( DotP, min[ 0 ])
			max[ 0 ] = Max( DotP, max[ 0 ])
		Next
	End Method
	
	
	#Rem
	Summary: Help function for calculate distance between bboxes of groups
	#End
	Method IntervalDistance:Float(MinA:Float[], MaxA:Float[], MinB:Float[], MaxB:Float[])
	
		Local res:Float
		
		If MinA[ 0 ] < MinB[ 0 ]
			res =  MinB[ 0 ] - MaxA[ 0 ]
		Else
			res =  MinA[ 0 ] - MaxB[ 0 ]
		Endif
		
		Return res	
	End Method
	
	Method BBoxOverlap : Bool( A : TBBox, B : TBBox )
			Return ( A.Min.x <= B.Max.x ) And ( A.Min.y <= B.Max.y ) And ( A.Max.x >= B.Min.x ) And ( B.Max.y >= A.Min.y )
			
			'Return (B.Min.x <= A.Max.x) And (A.Max.x >= B.Min.x) And (B.Min.y <= A.Max.y) And (A.Max.y >= B.Min.y)
			'Local c:TVector2 = VecSub(B2.center,B2.center)-VecAdd(B1.)
			'Local D:Float = |centreB-centreA| – (halfExtentsA+halfExtentsB)

			'binary overlap = D_x < 0 && D_y < 0;			
			
	End Method
	
	Method DetectPrimitiveCollision:TCollisionResult(polygonA:TPrimitive,polygonB:TPrimitive,velocity:TVector2)
	
		Local result:TCollisionResult = New TCollisionResult()
		result.Intersect = True
		result.WillIntersect = True
		
'		If BBoxOverlap(polygonA.bbox,polygonB.bbox)=False Then
'			result.Intersect = False
'			result.WillIntersect = False
'			result.normal.Set(0,0)
'			result.MinimumTranslationVector.Set(0,0)
'			Return result
'		End If		
		
		Local edgeCountA:Int = polygonA.edges.Size()
		Local edgeCountB:Int = polygonB.edges.Size()
		
		Local minIntervalDistance:Float = 100000.0
		Local translationAxis:TVector2 = New TVector2()
		Local edge:TEdge
		
		' Loop through all the edges of both polygons
		
		For Local edgeIndex:Int = 0 To (edgeCountA + edgeCountB)-1
			If (edgeIndex < edgeCountA) Then
				edge = polygonA.edges.Get(edgeIndex)	
			Else
				edge = polygonB.edges.Get(edgeIndex-edgeCountA)	
			End If
			
			' ===== 1. Find if the polygons are currently intersecting =====

			Local axis : TVector2 = edge.normal
			
			' Find the projection of the polygon on the current axis
			Local minA : Float[ 1 ]
			Local minB : Float[ 1 ]
			Local maxA : Float[ 1 ]
			Local maxB : Float[ 1 ]
			
			Self.ProjectPolygon(axis,polygonA,minA,maxA)			
			Self.ProjectPolygon(axis,polygonB,minB,maxB)
			
			result.normal = axis
			
			' Check if the polygon projections are currentlty intersecting

			if (Self.IntervalDistance(minA, maxA, minB, maxB) > 0) Then  result.Intersect = False
			
			
			' ===== 2. Now find if the polygons *will* intersect =====

			' Project the velocity on the current axis
			Local velocityProjection:Float = VecDot(axis,velocity)
				
			' Get the projection of polygon A during the movement
			If (velocityProjection < 0.0)  Then
			
					minA[0] += velocityProjection;
			Else
					maxA[0] += velocityProjection;
			EndIf
			
			' Do the same test as above for the new projection
			Local intervalDistance:Float = Self.IntervalDistance(minA, maxA, minB, maxB)
			If (intervalDistance > 0.0) Then result.WillIntersect = False
			
			' If the polygons are not intersecting and won't intersect, exit the loop
			If ( Not result.Intersect) And ( Not result.WillIntersect) Then			
				Return result
			End If
			
			' Check if the current interval distance is the minimum one. If so store
			' the interval distance and the current distance.
			' This will be used to calculate the minimum translation vector
			intervalDistance = Abs(intervalDistance)
			If (intervalDistance < minIntervalDistance) Then
				minIntervalDistance = intervalDistance
				translationAxis = axis
				Local d:TVector2 = VecSub(polygonA.center,polygonB.center)
				
				if (VecDot(d,translationAxis) < 0) Then translationAxis = VecMult(translationAxis,-1.0)
				
			EndIf
					
		Next
		
		' The minimum translation vector can be used to push the polygons appart.
		' First moves the polygons by their velocity
		' then move polygonA by MinimumTranslationVector.
		if (result.WillIntersect) Then 
			result.MinimumTranslationVector = VecMult(translationAxis,minIntervalDistance)
		EndIf
		
		return result
			
	End Method

End
